#include "gpuPlaneSpokeDart.h"
#include "cuda_defs.h"
#include "curand_kernel.h"
#include "randUtilGpu.h"
#include "randUtil.h"
#include "device_launch_parameters.h"
#include <algorithm>

#define INVALID_INDEX		0xFFFFFFFF
#define OUTSIDE_BOUNDARY	0xFFFFFFFE

namespace GPUPlaneSpokeDartCuda
{
__device__ __constant__ DATA_TYPE c_radius;
__device__ __constant__ DATA_TYPE c_sqRadius;
__device__ __constant__ DATA_TYPE c_radius_small;
__device__ __constant__ DATA_TYPE c_sqRadius_small;
__device__ __constant__ unsigned int c_m;
__device__ __constant__ DATA_TYPE c_invNumDimensions;

__device__ unsigned int d_checkedSamples[NUM_BLOCKS];
__device__ unsigned int d_front[MAX_FRONT];
__device__ DATA_TYPE2 d_intersectionArray[NUM_BLOCKS*NUM_THREADS*MAX_INTERVAL];
__device__ unsigned int d_numOverlaps[NUM_BLOCKS*NUM_THREADS];
__device__ int d_lock = 0;
__device__ unsigned char d_hasSolution[NUM_BLOCKS*NUM_THREADS];

// plane spoke loop data
__device__ unsigned int d_isConflict[NUM_BLOCKS*NUM_THREADS];

__device__ void discSphereIntersection(DATA_TYPE* p, DATA_TYPE* q, VectorGPU& dir1, VectorGPU& dir2, 
									   VectorGPU& dir3, DATA_TYPE& u, DATA_TYPE& v)
{
	DATA_TYPE proj[DIMENSIONS];
	DATA_TYPE qp[DIMENSIONS];

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		qp[d] = p[d] - q[d];

	// decompose qp using e1
	DATA_TYPE dot(0.0);
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		dot += qp[d] * dir1[d];
	}
	for (unsigned int d = 0; d < DIMENSIONS; ++d) 
		qp[d] -= dot * dir1[d];

	// decompose qp using e2
	dot = 0.0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d) 
	{
		dot += qp[d] * dir2[d];
	}
	for (unsigned int d = 0; d < DIMENSIONS; ++d)  
		qp[d] -= dot * dir2[d];

	// projection of p into the plane defined by e1 and e2
	for (unsigned int d = 0; d < DIMENSIONS; ++d) 
		proj[d] = p[d] - qp[d];

	DATA_TYPE proj_norm = 0.0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d) 
	{		
		DATA_TYPE dx = proj[d] - p[d];
		proj_norm += dx * dx;
	}
	DATA_TYPE r_proj = sqrt(c_sqRadius - proj_norm);

	DATA_TYPE H = 0.0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d) 
	{
		dir3[d] = proj[d] - q[d];
		H += dir3[d] * dir3[d];
	}

	if (H > (c_radius + r_proj) * (c_radius + r_proj)) 
	{
		return; // sphere do not intersect disc
	}

	H = sqrt(H);
	u = (c_sqRadius - r_proj * r_proj + H * H) / (2 * H); // distance of power splitter from first sphere
	v = sqrt(c_sqRadius - u * u);

	H = 1.0 / H;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		dir3[d] *= H; // normalizing e3

	return;
}

__device__ void discHyperplaneIntersection(bool minCorner, VectorGPU& n, DATA_TYPE* q, VectorGPU& dir1, VectorGPU& dir2, 
	VectorGPU& dir3, DATA_TYPE& u, DATA_TYPE& v)
{
	DATA_TYPE a(0.0), b(0.0), c(0.0);
	DATA_TYPE p = minCorner ? 0.0 : 1.0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		a += (q[d] - p) * n[d];
		b += dir1[d] * n[d];
		c += dir2[d] * n[d];
	}
	DATA_TYPE a1(0.0), a2(0.0); //  A = q + a1 e1 + a2 * e2 // a point on that line
	if (fabs(b) > fabs(c))
	{
		a2 = 0.0; 
		a1 = -a / b;
	}
	else
	{
		a1 = 0.0; 
		a2 = -a / c;
	}
	DATA_TYPE nx(b), ny(c); // normal directions to the intersecton line
	DATA_TYPE norm = sqrt(nx * nx + ny * ny);
	nx /= norm; ny /= norm;
	// project disk center on that line
	u = a1 * nx + a2 * ny;
	if (fabs(u) > c_radius_small) 
		return;

	v = sqrt(c_sqRadius - u * u);
	// e3 = nx e1 + ny e1
	DATA_TYPE dot = 0.0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		dir3[d] = nx * dir1[d] + ny * dir2[d];
		dot += dir3[d] * n[d];
	}
	if (dot < 0.0)
	{
		// reverse e3 and u
		u = -u;
		for (unsigned int d = 0; d < DIMENSIONS; ++d) 
			dir3[d] = -dir3[d];
	}
	return; // sphere do not intersect disc
}
////////////////////////////////////////

__global__ void initRandomStates(curandState* randStates, unsigned int* pNextFront, GPUDart::GPUMDVector samples, VectorGPU* pFirstSample)
{
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;

	curandState* localState = randStates + id;
	curand_init(RANDOM_SEED, id, 0, localState);

	if (id == 0)
	{
		d_front[0] = 0;
		*pNextFront = 1;

		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			samples(0, d) = (*pFirstSample)[d];
		//printf("First sample : %f %f\n", samples(0, 0), samples(0, 1));
	}
}

__global__ void setupGPUDarts(unsigned int* pNumFronts, unsigned int *d_numSamples, GPUDart::GPUMDVector samples, curandState* randStates, GPUDart::GPUMDVector refPoints, GPUDart::GPUMDVector directions,
	GPUDart::GPUInterval* intervals, GPUDart::GPUMDVector lastDarts, GPUDart::GPUMDVector directions1, GPUDart::GPUMDVector directions2)
{
		unsigned int numFronts = *pNumFronts;
	if (numFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;
	unsigned int trialId = id / numFronts;
	unsigned int frontId = id - trialId * numFronts;

	unsigned int numSamples = *d_numSamples;
	if (threadIdx.x == 0)
		d_checkedSamples[blockIdx.x] = numSamples;

	curandState* localState = randStates + id;

	unsigned int frontSampleIndex = d_front[frontId];
	DATA_TYPE refPoint[DIMENSIONS];
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		refPoint[d] = samples(frontSampleIndex, d);

	if (trialId == 0)
	{
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			refPoints(frontId, d) = refPoint[d];
	}

	VectorGPU direction1;
	VectorGPU direction2;

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		direction1[d] = RandUtilGpu::randGaussian(localState);
		direction2[d] = RandUtilGpu::randGaussian(localState);
	}
	direction1.normalize();
	direction2.normalize();

	VectorGPU dart1, dart2;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		dart1[d] = refPoint[d] + direction1[d] * c_radius;
		dart2[d] = refPoint[d] + direction2[d] * c_radius;
	}

	// Are darts in other disks?
	unsigned int conflictingSampleIndex1 = INVALID_INDEX;
	unsigned int conflictingSampleIndex2 = INVALID_INDEX;

	__shared__ DATA_TYPE sharedSamples[DIMENSIONS][NUM_THREADS];
	DATA_TYPE localCache[DIMENSIONS];

	// load samples to register
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		localCache[d] = samples(threadIdx.x, d);

	for (unsigned int j = 0; j < numSamples; )
	{
		// copy register to shared memory
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			sharedSamples[d][threadIdx.x] = localCache[d];

		__syncthreads();

		j += NUM_THREADS;

		// load samples to register
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			localCache[d] = samples(j + threadIdx.x, d);

		int numIterationSamples = min(NUM_THREADS, numSamples - (j - NUM_THREADS));

		for (unsigned int i = 0; i < numIterationSamples; ++i)
		{
			if (j - NUM_THREADS + i != frontSampleIndex)
			{
				DATA_TYPE sqDist1 = 0;
				DATA_TYPE sqDist2 = 0;
				DATA_TYPE diff;
				for (unsigned int d = 0; d < DIMENSIONS; ++d)
				{
					diff = dart1[d] - sharedSamples[d][i];
					sqDist1 += diff * diff;
					diff = dart2[d] - sharedSamples[d][i];
					sqDist2 += diff * diff;
				}
				if (sqDist1 < c_sqRadius_small)
				{
					conflictingSampleIndex1 = (j - NUM_THREADS) + i;
				}
				if (sqDist2 < c_sqRadius_small)
				{
					conflictingSampleIndex2 = (j - NUM_THREADS) + i;
				}
			}
		}
		__syncthreads();
	}

	// Is dart in domain?
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		if (dart1[d] < -DATA_TYPE_EPS || dart1[d] > 1.0f + DATA_TYPE_EPS)
			conflictingSampleIndex1 = OUTSIDE_BOUNDARY;
		if (dart2[d] < -DATA_TYPE_EPS || dart2[d] > 1.0f + DATA_TYPE_EPS)
			conflictingSampleIndex2 = OUTSIDE_BOUNDARY;
	}

	unsigned int isConflict = INVALID_INDEX;
	VectorGPU* src = &direction1;
	if (conflictingSampleIndex1 != INVALID_INDEX)
	{
		if (conflictingSampleIndex2 != INVALID_INDEX)
		{
			isConflict = conflictingSampleIndex1;
		}
		else
		{
			src = &direction2;
		}
	}

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		directions1(id, d) = direction1[d];
		directions2(id, d) = direction2[d];
	}

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		directions(id, d) = (*src)[d];

	d_isConflict[id] = isConflict;
}


__global__ void spokeDartLoop(unsigned int* pNumFronts, unsigned int *d_numSamples, GPUDart::GPUMDVector samples, curandState* randStates, GPUDart::GPUMDVector refPoints, GPUDart::GPUMDVector directions,
	GPUDart::GPUInterval* intervals, GPUDart::GPUMDVector lastDarts, GPUDart::GPUMDVector directions1, GPUDart::GPUMDVector directions2)
{
	unsigned int numFronts = *pNumFronts;
	if (numFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;
	unsigned int trialId = id / numFronts;
	unsigned int frontId = id - trialId * numFronts;

	unsigned int numSamples = d_checkedSamples[blockIdx.x];
	unsigned int frontSampleIndex = d_front[frontId];

	DATA_TYPE refPoint[DIMENSIONS];

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		refPoint[d] = refPoints(frontId, d);

	unsigned int conflictingSampleIndex1 = d_isConflict[id];
	DATA_TYPE conflictingSample1[DIMENSIONS];
	__shared__ DATA_TYPE sharedSamples[DIMENSIONS][NUM_THREADS];
	DATA_TYPE localCache[DIMENSIONS];

	VectorGPU direction1;
	VectorGPU direction2;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		direction1[d] = directions1(id, d);
		direction2[d] = directions2(id, d);

		if (conflictingSampleIndex1 < OUTSIDE_BOUNDARY)
			conflictingSample1[d] = samples(conflictingSampleIndex1, d);
	}	
	// orthonormalize dir2
	VectorGPU d2backup = direction2;
	direction2 = direction2 - (direction1 * direction2) * direction1;
	direction2.normalize();

	VectorGPU dart1;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		dart1[d] = refPoint[d] + direction1[d] * c_radius;
	}

	DATA_TYPE thetaOld = 0;
	DATA_TYPE2 theta = {0, 0};
	bool first = true;
	bool needUpdate = (conflictingSampleIndex1 != INVALID_INDEX);
	bool done = !needUpdate;
	unsigned char loopCounter = 0;
	while (1)
	{
		if (!done)
		{
			// domain check
			VectorGPU n;
			bool minCorner = true;
			bool boundaryFound = false;
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
			{
				n[d] = 0.0f;
				if (!boundaryFound)
				{
					if (dart1[d] < -DATA_TYPE_EPS)
					{
						n[d] = -1.0f;
						minCorner = true;
						boundaryFound = true;
					}
					else if (dart1[d] > 1.0f + DATA_TYPE_EPS)
					{
						n[d] = 1.0f;
						minCorner = false;
						boundaryFound = true;
					}
				}
			}
			if (conflictingSampleIndex1 == OUTSIDE_BOUNDARY && !boundaryFound)
				printf("zero normal!!!\n");

			VectorGPU direction3;
			DATA_TYPE u, v;
			if (conflictingSampleIndex1 == OUTSIDE_BOUNDARY)
			{
				// disc-hyperplane intersection
				discHyperplaneIntersection(minCorner, n, refPoint, direction1, direction2, direction3, u, v);

				// for debug
				if (u != u || v != v)
					printf("NaN plane u, v dart:(%f %f %f %f) n:(%f %f %f %f) d1:(%f %f %f %f) d2:(%f %f %f %f)\n", 
					dart1[0], dart1[1], dart1[2], dart1[3],
					n[0], n[1], n[2], n[3],
					direction1[0],direction1[1],direction1[2],direction1[3],
					d2backup[0],d2backup[1],d2backup[2],d2backup[3]);
			}
			else
			{
				// disc-sphere intersection
				discSphereIntersection(conflictingSample1, refPoint, direction1, direction2, direction3, u, v);

				// for debug
				if (u != u || v != v)
					printf("NaN sphere u, v dart:(%f %f %f %f) ref:(%f %f %f %f) conf:(%f %f %f %f) d1:(%f %f %f %f) d2:(%f %f %f %f)\n", 
					dart1[0], dart1[1], dart1[2], dart1[3],
					refPoint[0], refPoint[1], refPoint[2], refPoint[3],
					conflictingSample1[0], conflictingSample1[1], conflictingSample1[2], conflictingSample1[3],
					direction1[0],direction1[1],direction1[2],direction1[3],
					d2backup[0],d2backup[1],d2backup[2],d2backup[3]);
			}

			DATA_TYPE cosTheta = direction3 * direction1;
			DATA_TYPE sinTheta = direction3 * direction2;

			thetaOld = theta.y;
			theta.y = atan2(sinTheta, cosTheta) + PI - theta.x; // angle between 0 and 2 PI
			if (theta.y < 0.0) 
				theta.y += 2 * PI;

			if (first)
			{
				theta.x = theta.y;
				theta.y = 0;
				first = false;
			}
			else
			{
				DATA_TYPE dtheta = theta.y - thetaOld;
				if (dtheta < 0) 
					done = true;
			}

			DATA_TYPE u1 = u * cosTheta - v * sinTheta;
			DATA_TYPE v1 = u * sinTheta + v * cosTheta;

			// set direction1 as new intersection point
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
			{
				dart1[d] = refPoint[d] + u1 * direction1[d] + v1 * direction2[d];
			}
		}

		// check validity of the new point
		{
			// Is dart in other disks?
			conflictingSampleIndex1 = INVALID_INDEX;

			// load samples to register
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				localCache[d] = samples(threadIdx.x, d);

			for (unsigned int j = 0; j < numSamples; )
			{
				// copy register to shared memory
				for (unsigned int d = 0; d < DIMENSIONS; ++d)
					sharedSamples[d][threadIdx.x] = localCache[d];

				__syncthreads();

				j += NUM_THREADS;

				// load samples to register
				for (unsigned int d = 0; d < DIMENSIONS; ++d)
					localCache[d] = samples(j + threadIdx.x, d);

				int numIterationSamples = min(NUM_THREADS, numSamples - (j - NUM_THREADS));

				for (unsigned int i = 0; i < numIterationSamples; ++i)
				{
					if (j - NUM_THREADS + i != frontSampleIndex)
					{
						DATA_TYPE sqDist1 = 0;
						DATA_TYPE diff;
						for (unsigned int d = 0; d < DIMENSIONS; ++d)
						{
							diff = dart1[d] - sharedSamples[d][i];
							sqDist1 += diff * diff;
						}
						if (sqDist1 < c_sqRadius_small)
						{
							conflictingSampleIndex1 = (j - NUM_THREADS) + i;
							for (unsigned int d = 0; d < DIMENSIONS; ++d)
								conflictingSample1[d] = sharedSamples[d][i];
						}
					}
				}
				__syncthreads();
			}

			// Is dart in domain?
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
			{
				if (dart1[d] < -DATA_TYPE_EPS || dart1[d] > 1.0f + DATA_TYPE_EPS)
					conflictingSampleIndex1 = OUTSIDE_BOUNDARY;
			}
		}

		// for debug
		if (++loopCounter > 50 && !done)
		{
			printf("infinite loop!!! %d (%f %f %f %f)\n", conflictingSampleIndex1, dart1[0], dart1[1], dart1[2], dart1[3]);
			done = true;
		}

		// if it is valid, done
		if (conflictingSampleIndex1 == INVALID_INDEX)
		{
			//if (!done && loopCounter > 10)
			//printf("loop exit after %d iterations\n", loopCounter);
			done = true;
		}

		// if all threads in a warp are done, exit loop
		if (__syncthreads_and(done))
			break;
	}

	if (needUpdate)
	{
		VectorGPU newDirection;
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			newDirection[d] = dart1[d] - refPoint[d];
		newDirection.normalize();
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			directions(id, d) = newDirection[d];
	}
}

__global__ void updateIntervalsKernel(unsigned int* pNumFronts, GPUDart::GPUMDVector samples, GPUPlaneSpokeDart::GPUInterval* intervals,
	unsigned int *d_numSamples, GPUDart::GPUMDVector refPoints, GPUDart::GPUMDVector directions)
{
	unsigned int numFronts = *pNumFronts;
	if (numFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;
	unsigned int frontId = id - (id / numFronts) * numFronts;

	unsigned int numSamples = d_checkedSamples[blockIdx.x];//*d_numSamples;

	//if (threadIdx.x == 0)
		//d_checkedSamples[blockIdx.x] = numSamples;

	DATA_TYPE refPoint[DIMENSIONS];
	DATA_TYPE direction[DIMENSIONS];

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		refPoint[d] = refPoints(frontId, d);
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		direction[d] = directions(id, d);
	
	DATA_TYPE2* __restrict__ intersectionArray = d_intersectionArray + id;
	unsigned int numOverlaps = 0;

	__shared__ DATA_TYPE sharedSamples[DIMENSIONS][NUM_THREADS];
	DATA_TYPE localCache[DIMENSIONS];
	
	// load samples to register
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		localCache[d] = samples(threadIdx.x, d);
	
	for (unsigned int j = 0; j < numSamples; )
	{
		// copy register to shared memory
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			sharedSamples[d][threadIdx.x] = localCache[d];

		__syncthreads();

		j += NUM_THREADS;

		// load samples to register
		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			localCache[d] = samples(j + threadIdx.x, d);

		int numIterationSamples = min(NUM_THREADS, numSamples - (j - NUM_THREADS));
		
		for (unsigned int i = 0; i < numIterationSamples; ++i)
		{
			// ray-sphere intersection
			// Compute A, B and C coefficients
			DATA_TYPE oc[DIMENSIONS];
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				oc[d] = refPoint[d] - sharedSamples[d][i];

			DATA_TYPE a = 0;
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				a += direction[d] * direction[d];

			DATA_TYPE b = 0;
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				b += oc[d] * direction[d];
			b *= 2;

			DATA_TYPE c = -c_sqRadius;
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				c += oc[d] * oc[d];

			//Find discriminant
			DATA_TYPE disc = b * b - 4 * a * c;

			// if discriminant is negative there are no real roots, so return 
			// false as ray misses sphere
			if (disc >= 0)
			{
				intersectionArray[numOverlaps * NUM_THREADS * NUM_BLOCKS].x = b;
				intersectionArray[numOverlaps * NUM_THREADS * NUM_BLOCKS].y = c;
				++numOverlaps;
			}
		}
		__syncthreads();
	}

	d_numOverlaps[id] = numOverlaps;
}

__global__ void updateIntervals2Kernel(unsigned int* pNumFronts, GPUDart::GPUMDVector samples, GPUPlaneSpokeDart::GPUInterval* intervals,
	unsigned int *d_numSamples, GPUDart::GPUMDVector refPoints, GPUDart::GPUMDVector directions)
{
	unsigned int numFronts = *pNumFronts;
	if (numFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;
	unsigned int frontId = id - (id / numFronts) * numFronts;

	DATA_TYPE refPoint[DIMENSIONS];
	DATA_TYPE direction[DIMENSIONS];
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		refPoint[d] = refPoints(frontId, d);
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		direction[d] = directions(id, d);

	// clamp interval at domain boundaries
	GPUDart::GPUInterval& interval = intervals[id];
	DATA_TYPE intervalMin = -2 * c_radius;
	DATA_TYPE intervalMax = 2 * c_radius;
	for (unsigned int i = 0; i < DIMENSIONS; ++i)
	{
		if (direction[i] > 0)
		{
			DATA_TYPE t = ((DATA_TYPE)1 - refPoint[i]) / direction[i];
			intervalMax = min(intervalMax, t);

			t = (0 - refPoint[i]) / direction[i];
			intervalMin = max(intervalMin, t);
		}
		else if (direction[i] < 0)
		{
			DATA_TYPE t = (0 - refPoint[i]) / direction[i];
			intervalMax = min(intervalMax, t);

			t = ((DATA_TYPE)1 - refPoint[i]) / direction[i];
			intervalMin = max(intervalMin, t);
		}
	}
	interval.prev[0] = -1;
	interval.head = 0;
	interval.tail = 0;
	interval.numIntervals = 0;
	interval.numUsed = 1;
	if (intervalMin < -c_radius)
		interval.insert(interval.tail, intervalMin, -c_radius);
	if (intervalMax > c_radius)
		interval.insert(interval.tail, c_radius, intervalMax);

	DATA_TYPE2* __restrict__ intersectionArray = d_intersectionArray + id;
	unsigned int numOverlaps = d_numOverlaps[id];

	double a = 0;
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		a += direction[d] * direction[d];

	for (unsigned int j = 0; j < numOverlaps; ++j)
	{
		// ray-sphere intersection continues
		double b = intersectionArray->x;
		double c = intersectionArray->y;

		intersectionArray += NUM_BLOCKS * NUM_THREADS;

		// use double precision. 
		// single presion is not enough here.
		double distSqrt = sqrt(b * b - 4 * a * c);
		double q;
		if (b < 0)
			q = (-b - distSqrt) / 2;
		else
			q = (-b + distSqrt) / 2;

		// compute t0 and t1
		DATA_TYPE x0 = (DATA_TYPE)(q / a);
		DATA_TYPE x1 = (DATA_TYPE)(c / q);
		if (x0 > x1)
		{
			// if t0 is bigger than t1 swap them around
			DATA_TYPE temp = x0;
			x0 = x1;
			x1 = temp;
		}

		short distance = interval.numIntervals;
		short low, up, tempIt;
		low = GPUDart::GPUInterval::lower_bound(interval.intervalBegins, interval.next, interval.head, interval.tail, x0, &distance);
		tempIt = low;
		if (low != interval.head)
		{
			tempIt = interval.prev[low];
			distance += 1;
		}
		up = GPUDart::GPUInterval::upper_bound(interval.intervalEnds, interval.next, tempIt, interval.tail, x1, &distance);

		DATA_TYPE min = interval.intervalBegins[interval.head];
		DATA_TYPE max = interval.intervalEnds[interval.prev[interval.tail]];

		tempIt = up;
		// split an interval
		if (up != interval.tail && interval.next[tempIt] == low)
		{
			DATA_TYPE splitEnd = interval.intervalEnds[up];
			interval.intervalEnds[up] = x0;
			interval.insert(low, x1, splitEnd);
		}
		else
		{
			if (x0 > min)
			{
				tempIt = interval.prev[low];
				if (x0 < interval.intervalEnds[tempIt])
					interval.intervalEnds[tempIt] = x0;
			}
			if (x1 < max)
			{
				if (x1 > interval.intervalBegins[up])
					interval.intervalBegins[up] = x1;
			}
			interval.erase(low, up);
		}
	} 
}

__device__ DATA_TYPE raisePower(DATA_TYPE value)
{
	bool invert = (value < 0);
	if (invert) 
		value = -value;
	value = pow(value, (DATA_TYPE)DIMENSIONS);
	if (invert)
		value = -value;
	return value;
}

__device__ DATA_TYPE lowerPower(DATA_TYPE value)
{
	bool invert = (value < 0);
	if (invert) 
		value = -value;
	value = pow(value, c_invNumDimensions);
	if (invert)
		value = -value;
	return value;
}

__global__ void pickPointKernel(unsigned int* pNumFronts, curandState* randStates, GPUPlaneSpokeDart::GPUInterval* intervals, 
		GPUDart::GPUMDVector refPoints, GPUDart::GPUMDVector directions, GPUDart::GPUMDVector computedSamples)
{
	unsigned int numFronts = *pNumFronts;
	if (numFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;
	unsigned int frontId = id - (id / numFronts) * numFronts;

	GPUDart::GPUInterval& interval = intervals[id];
	DATA_TYPE refPoint[DIMENSIONS];
	DATA_TYPE direction[DIMENSIONS];

	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		refPoint[d] = refPoints(frontId, d);
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		direction[d] = directions(id, d);

	short numIntervals = interval.numIntervals;
	d_hasSolution[id] = (numIntervals != 0) ? 1 : 0;
	if (numIntervals != 0)
	{
		for (int i = interval.head; i != interval.tail; i = interval.next[i])
		{
			interval.intervalBegins[i] = raisePower(interval.intervalBegins[i]);
			interval.intervalEnds[i] = raisePower(interval.intervalEnds[i]);
		}

		DATA_TYPE totalLength = 0;
		for (int i = interval.head; i != interval.tail; i = interval.next[i])
		{
			totalLength += interval.intervalEnds[i] - interval.intervalBegins[i];
		}

		curandState* localState = randStates + id;
		DATA_TYPE randPoint = RandUtilGpu::randRange(localState, totalLength);

		DATA_TYPE computedSample[DIMENSIONS];
		for (int i = interval.head; i != interval.tail; i = interval.next[i])
		{
			DATA_TYPE length = interval.intervalEnds[i] - interval.intervalBegins[i];
			if (randPoint <= length)
			{
				DATA_TYPE t = lowerPower(interval.intervalBegins[i] + randPoint);
				for (unsigned int d = 0; d < DIMENSIONS; ++d)
					computedSample[d] = refPoint[d] + t * direction[d];

				//printf("[%d] t : %f (%f %f) = (%f %f) * %f (%f %f)\n", id, t, computedSample[0], computedSample[1], refPoint[0], refPoint[1], t, direction[0], direction[1]);
				break;
			}
			randPoint -= length;
		}

		for (unsigned int d = 0; d < DIMENSIONS; ++d)
			computedSamples(id, d) = computedSample[d];

		//printf("[%d] picked (%f, %f)\n", id, computedSample[0], computedSample[1]);
	}
}

__global__ void addSamplesKernel(unsigned int* pNumFronts, GPUDart::GPUMDVector samples, 
		unsigned int *pNumSamples, unsigned int* pFailureCounts, GPUDart::GPUMDVector computedSamples)
{
	if (*pNumFronts == 0)
		return;
	unsigned int id = threadIdx.x + blockIdx.x * blockDim.x;

	__shared__ unsigned char sharedHasSample[NUM_THREADS];
	__shared__ VectorGPU sharedSamples[NUM_THREADS];

	sharedHasSample[threadIdx.x] = d_hasSolution[id];
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
		sharedSamples[threadIdx.x][d] = computedSamples(id, d);

	__syncthreads();

	VectorGPU diff;

	// discard in block
	for (int i = 0; i < blockDim.x; ++i)
	{
		if (i < threadIdx.x && sharedHasSample[i] && sharedHasSample[threadIdx.x])
		{
			VectorGPU otherSample = sharedSamples[i];
			VectorGPU thisSample = sharedSamples[threadIdx.x];
			for (int d = 0; d < DIMENSIONS; ++d)
				diff[d] = otherSample[d] - thisSample[d];
			if (diff.getSquaredNorm() < c_sqRadius)
			{
				sharedHasSample[threadIdx.x] = false;
				//printf("[%d] discarded\n", threadIdx.x);
				break;
			}
		}
	}

	__syncthreads();

	if (threadIdx.x == 0)
	{
		unsigned int numCheckedSamples = d_checkedSamples[blockIdx.x];
		unsigned int numBlockSamples = 0;
		unsigned int blockSampleIndices[NUM_THREADS];
		for (unsigned int s = 0; s < NUM_THREADS; ++s)
		{
			if (sharedHasSample[s])
			{
				blockSampleIndices[numBlockSamples++] = s;
			}
		}

		unsigned int numSamples;
		unsigned int numUncheckedSamples;

		// acquire lock
		while (atomicExch(&d_lock, 1) != 0)
		{
		}
		numSamples = *pNumSamples;
		numUncheckedSamples = numSamples;

		if (numUncheckedSamples != numCheckedSamples)
		{
			for (unsigned int i = numCheckedSamples; i < numUncheckedSamples; ++i)
			{
				VectorGPU sample;
				for (unsigned int d = 0; d < DIMENSIONS; ++d)
					sample[d] = samples(i, d);
				for (unsigned int s = 0; s < numBlockSamples; )
				{
					unsigned int sampleIndex = blockSampleIndices[s];
					VectorGPU refPoint = sharedSamples[sampleIndex];

					for (unsigned int d = 0; d < DIMENSIONS; ++d)
						diff[d] = sample[d] - refPoint[d];

					if (diff.getSquaredNorm() < c_sqRadius)
					{
						if (s != numBlockSamples - 1)
							blockSampleIndices[s] = blockSampleIndices[numBlockSamples - 1];
						--numBlockSamples;
					}
					else
					{
						++s;
					}
				}
			}
			numCheckedSamples = numUncheckedSamples;
		}

		for (unsigned int s = 0; s < numBlockSamples; ++s)
		{
			unsigned int sampleIndex = blockSampleIndices[s];
			VectorGPU newSample = sharedSamples[sampleIndex];
			for (unsigned int d = 0; d < DIMENSIONS; ++d)
				samples(numSamples, d) = newSample[d];

				//printf("[%d] adds (%f, %f) as sample %d\n", sampleIndex, newSample[0], newSample[1], numSamples);
			++numSamples;
		}

		(*pNumSamples) = numSamples;

		// release lock
		atomicExch(&d_lock, 0);
	}
}


__global__ void updateFront(unsigned int* pNumFronts, unsigned int *pNumSamples, unsigned int* pFailureCounts, unsigned int* pNextFront, unsigned int numBlocks,
	GPUDart::GPUMDVector lastDarts, GPUDart::GPUMDVector directions1, GPUDart::GPUMDVector directions2)
{
	unsigned int frontId = threadIdx.x;

	unsigned int numFronts = *pNumFronts;
	unsigned int numSamples = *pNumSamples;

	if (numFronts == 0)
		return;

	__shared__ unsigned int sharedFronts[NUM_BLOCKS * NUM_THREADS];

	// update failure count & advance front
	for (; frontId < numFronts; frontId += blockDim.x)
	{
		sharedFronts[frontId] = d_front[frontId];
		unsigned int failureCount = pFailureCounts[frontId];

		for (unsigned int i = frontId; i < numBlocks * NUM_THREADS; i += numFronts)
		{
			unsigned char trialHasSolution = d_hasSolution[frontId];
			if (trialHasSolution)
			{
				failureCount = 0;
			}
			else
				++failureCount;
		}

		if (failureCount >= c_m)
		{
			// for debug only
			//unsigned int old = sharedFronts[frontId];

			sharedFronts[frontId] = atomicInc(pNextFront, INT_MAX);
			failureCount = 0;

			//printf("replaced front [%d] : %d->%d\n", frontId, old, sharedFronts[frontId]);
		}
		pFailureCounts[frontId] = failureCount;
	}
	__syncthreads();

	// remove empty fronts
	unsigned int backup;
	if (threadIdx.x == 0)
	{
		for (unsigned int i = 0; i < numFronts; ++i)
		{
			if (sharedFronts[i] >= numSamples)
			{
				--numFronts;
				if (i != numFronts)
				{
					// copy last data to i-th data
					//printf("front [%d] removed : %d\n", i, sharedFronts[i]);
					sharedFronts[i] = sharedFronts[numFronts];
					pFailureCounts[i] = pFailureCounts[numFronts];

					--i;
				}
				pFailureCounts[numFronts] = 0;
			}
		}
		backup = sharedFronts[0];
		sharedFronts[0] = numFronts;
	}
	__syncthreads();
	if (threadIdx.x != 0)
		numFronts = sharedFronts[0];
	__syncthreads();
	if (threadIdx.x == 0)
		sharedFronts[0] = backup;

	// add new fronts
	unsigned int maxFront = numBlocks * NUM_THREADS;
	unsigned int nextFront = min(*pNextFront, numSamples);
	unsigned int numAddedFronts = min(numSamples - nextFront, maxFront - numFronts);
	for (frontId = numFronts + threadIdx.x; frontId < numFronts + numAddedFronts; frontId += blockDim.x)
	{
		sharedFronts[frontId] = nextFront + frontId - numFronts;
	}
	__syncthreads();

	numFronts += numAddedFronts;

	// write
	for (frontId = threadIdx.x; frontId < numFronts; frontId += blockDim.x)
	{
		d_front[frontId] = sharedFronts[frontId];
	}

	if (threadIdx.x == 0)
	{
		*pNumFronts = numFronts;
		*pNextFront = nextFront + numAddedFronts;
	}
}

};

////////////////////////////////////////////////////////////////////////////////

GPUPlaneSpokeDart::GPUPlaneSpokeDart()
{

}

GPUPlaneSpokeDart::~GPUPlaneSpokeDart()
{
	CPUFREE(h_buffer_);

	d_samples_.destroy();
	d_refPoints_.destroy();
	d_directions_.destroy();
	d_computedSamples_.destroy();
	d_lastDarts_.destroy();
	d_direction1_.destroy();
	d_direction2_.destroy();

	GPUFREE(d_randStates_);
	GPUFREE(d_intervals_);
	GPUFREE(d_numSamples_);
	GPUFREE(d_failureCount_);

}

bool GPUPlaneSpokeDart::init(unsigned int m, DATA_TYPE radius, unsigned int d)
{
	if (d != DIMENSIONS)
	{
		printf("Dimension mismatch!!!\n");
		return false;
	}
	

	cudaDeviceSetSharedMemConfig(cudaSharedMemBankSizeEightByte);

	h_m_ = m;
	h_radius_ = radius;
	h_dimensions_ = d;

	d_samples_.init(MAX_SAMPLES, h_dimensions_);
	d_refPoints_.init(MAX_FRONT, h_dimensions_);
	d_directions_.init(NUM_THREADS * NUM_BLOCKS, h_dimensions_);
	d_computedSamples_.init(NUM_THREADS * NUM_BLOCKS, h_dimensions_);
	d_lastDarts_.init(MAX_FRONT, h_dimensions_);
	d_direction1_.init(NUM_BLOCKS*NUM_THREADS, h_dimensions_);
	d_direction2_.init(NUM_BLOCKS*NUM_THREADS, h_dimensions_);

	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_m, &m, sizeof(unsigned int));
	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_radius, &radius, sizeof(DATA_TYPE));
	DATA_TYPE radius_small = radius - DATA_TYPE_EPS;
	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_radius_small, &radius_small, sizeof(DATA_TYPE));
	DATA_TYPE sqRadius = radius * radius;
	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_sqRadius, &sqRadius, sizeof(DATA_TYPE));
	DATA_TYPE sqRadius_small = sqRadius - DATA_TYPE_EPS;
	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_sqRadius_small, &sqRadius_small, sizeof(DATA_TYPE));
	DATA_TYPE invDimensions = (DATA_TYPE)1 / DIMENSIONS;
	TOGPU_CONSTANT(GPUPlaneSpokeDartCuda::c_invNumDimensions, &invDimensions, sizeof(DATA_TYPE));

	GPUMALLOC(&d_randStates_, sizeof(curandState) * NUM_THREADS * NUM_BLOCKS);
	GPUMALLOC(&d_intervals_, sizeof(GPUInterval) * NUM_THREADS * NUM_BLOCKS);
	GPUMALLOC(&d_numSamples_, sizeof(unsigned int) * 3);
	d_numFront_ = (unsigned int*)d_numSamples_ + 1;
	d_nextFront_ = (unsigned int*)d_numSamples_ + 2;
	GPUMALLOC(&d_failureCount_, sizeof(unsigned int) * MAX_FRONT);

	CPUMALLOC(&h_buffer_, sizeof(unsigned int) * 3);

	// add the first sample
	VectorGPU firstSample;
	VectorGPU* d_firstSample;
	GPUMALLOC(&d_firstSample, sizeof(VectorGPU));
	for (unsigned int d = 0; d < DIMENSIONS; ++d)
	{
		firstSample[d] = RandUtil::rand0To1();
	}
	TOGPU(d_firstSample, &firstSample, sizeof(VectorGPU));

	h_numSamples_ = 1;
	TOGPU((void*)d_numSamples_, &h_numSamples_, sizeof(unsigned int));

	h_numFront_ = 1;
	TOGPU((void*)d_numFront_, &h_numFront_, sizeof(unsigned int));
	GPUMEMSET(d_failureCount_, 0, sizeof(unsigned int) * MAX_FRONT);



	GPUPlaneSpokeDartCuda::initRandomStates <<< NUM_BLOCKS, NUM_THREADS >>> (d_randStates_, d_nextFront_, d_samples_, d_firstSample);

	CUDA_CHECK_ERROR();

	GPUFREE(d_firstSample);

	return true;
}

void GPUPlaneSpokeDart::setupDarts()
{
	int numBlocks = useMultipleBlocks_ ? NUM_BLOCKS : 1;
	GPUPlaneSpokeDartCuda::setupGPUDarts <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_numSamples_, d_samples_, d_randStates_, d_refPoints_, d_directions_, d_intervals_,
		d_lastDarts_, d_direction1_, d_direction2_);
	CUDA_CHECK_ERROR();

	GPUPlaneSpokeDartCuda::spokeDartLoop <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_numSamples_, d_samples_, d_randStates_, d_refPoints_, d_directions_, d_intervals_,
		d_lastDarts_, d_direction1_, d_direction2_);

	CUDA_CHECK_ERROR();
}

void GPUPlaneSpokeDart::updateIntervals()
{
	int numBlocks = useMultipleBlocks_ ? NUM_BLOCKS : 1;
	GPUPlaneSpokeDartCuda::updateIntervalsKernel <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_samples_, d_intervals_, d_numSamples_,
		d_refPoints_, d_directions_);

	CUDA_CHECK_ERROR();
}

void GPUPlaneSpokeDart::updateIntervals2()
{
	int numBlocks = useMultipleBlocks_ ? NUM_BLOCKS : 1;
	GPUPlaneSpokeDartCuda::updateIntervals2Kernel <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_samples_, d_intervals_, d_numSamples_,
		d_refPoints_, d_directions_);

	CUDA_CHECK_ERROR();
}

void GPUPlaneSpokeDart::pickPoint()
{
	int numBlocks = useMultipleBlocks_ ? NUM_BLOCKS : 1;
	GPUPlaneSpokeDartCuda::pickPointKernel <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_randStates_, d_intervals_, 
		d_refPoints_, d_directions_, d_computedSamples_);

	CUDA_CHECK_ERROR();
}

void GPUPlaneSpokeDart::addSamples()
{
	static int doSync = 100;

	if (useMultipleBlocks_ && doSync > 0)
		cudaDeviceSynchronize();

	int numBlocks = useMultipleBlocks_ ? NUM_BLOCKS : 1;
	GPUPlaneSpokeDartCuda::addSamplesKernel <<< numBlocks, NUM_THREADS >>> (d_numFront_, d_samples_, d_numSamples_,
		d_failureCount_, d_computedSamples_);

	if (useMultipleBlocks_ && doSync > 0)
	{
		cudaDeviceSynchronize();
		--doSync;
	}

	CUDA_CHECK_ERROR();

	GPUPlaneSpokeDartCuda::updateFront <<< 1, NUM_THREADS >>> (d_numFront_, d_numSamples_, d_failureCount_, d_nextFront_, numBlocks, d_lastDarts_, d_direction1_, d_direction2_);

	CUDA_CHECK_ERROR();
}

bool GPUPlaneSpokeDart::terminationCheck()
{
	// TODO:
	return h_numFront_ == 0;
}

unsigned int GPUPlaneSpokeDart::getNumGeneratedSamples()
{
	FROMGPU(h_buffer_, d_numSamples_, sizeof(unsigned int) * 3);
	h_numSamples_ = h_buffer_[0];
	h_numFront_ = h_buffer_[1];
	h_numNextFront_ = h_buffer_[2];
	//printf("# samples : %d # front : %d\n", h_numSamples_, h_numFront_);

	return h_numSamples_;
}

bool compareVectorGPUX(const VectorGPU& v1, const VectorGPU& v2){
        return v1[0] < v2[0];
}

void GPUPlaneSpokeDart::downloadSamples()
{
	FROMGPU(h_buffer_, (unsigned int*)d_numSamples_, sizeof(unsigned int));
	h_numSamples_ = h_buffer_[0];

	if (h_numSamples_ == 0)
		return;

	DATA_TYPE* h_temp = new DATA_TYPE[d_samples_.size_ * h_dimensions_];
	FROMGPU(h_temp, d_samples_.p_, sizeof(DATA_TYPE) * d_samples_.size_ * h_dimensions_);
	VectorGPU* h_samples = new VectorGPU[h_numSamples_];
	for (unsigned int i = 0; i < h_numSamples_; ++i)
	{
		for (unsigned int d = 0; d < h_dimensions_; ++d)
		{
			h_samples[i][d] = h_temp[d * d_samples_.size_ + i];
		}
	}

	/*
	for (int i = 0; i < h_numSamples_; ++i)
	{
		printf("[%d] ", i);
		for (int d = 0; d < h_dimensions_; ++d)
		{
			printf("%f ", h_samples[i].elements_[d]);
		}
		printf("\n");
	}
	*/

	if (validateResults_)
	{
		printf("validation\n");
		std::sort(h_samples, h_samples + h_numSamples_, compareVectorGPUX);
		for (unsigned int i = 0; i < h_numSamples_; ++i)
		{
			for (unsigned int j = i + 1; j < h_numSamples_; ++j)
			{
				if (h_samples[j][0] > h_samples[i][0] + h_radius_)
					break;

				DATA_TYPE sq = (h_samples[i] - h_samples[j]).getSquaredNorm();
				if (sq < h_radius_ * h_radius_ - DATA_TYPE_EPS)
				{
					printf("!!!invalid %d-%d dist : %f\n", i, j, sqrt(sq));
				}
			}
			if (i % 1000 == 0)
				printf("%d\n", i);
		}
	}

	printf("# of samples : %d\n", h_numSamples_);

	delete[] h_samples;
	delete[] h_temp;
}

////////////////////////////////////////////////////////////////////////////////

